#include "ygz/System.h"
#include "ygz/TrackerLK.h"
#include "ygz/BackendSlidingWindowG2O.h"

namespace ygz {

    System::System(const string &configPath) {

        // Read rectification parameters
        cv::FileStorage fsSettings(configPath, cv::FileStorage::READ);

        if (!fsSettings.isOpened()) {
            LOG(FATAL) << "ERROR: Wrong path to settings" << endl;
            return;
        }

        int rows_l = fsSettings["LEFT.height"];
        int cols_l = fsSettings["LEFT.width"];

        setting::imageHeight = rows_l;
        setting::imageWidth = cols_l;

        // Create camera object
        setting::initSettings();
        float fx = fsSettings["Camera.fx"];
        float fy = fsSettings["Camera.fy"];
        float cx = fsSettings["Camera.cx"];
        float cy = fsSettings["Camera.cy"];
        float bf = fsSettings["Camera.bf"];

        string method = fsSettings["System.method"];
        if (method == string("LK_FLOW"))
            mMethod = LK_FLOW;
        else if (method == string("ORB_FEATURE"))
            mMethod = ORB_FEATURE;
        else {
            LOG(ERROR) << "cannot read system method, use LK_FLOW as default" << endl;
            mMethod = LK_FLOW;
        }

        shared_ptr<CameraParam> camera(new CameraParam(fx, fy, cx, cy, bf));

        // create a tracker
        if (mMethod == LK_FLOW) {
            mpTracker = shared_ptr<TrackerLK>(new TrackerLK());
        } else {
            mpTracker = shared_ptr<Tracker>(new Tracker());
        }

        mpTracker->SetCamera(camera);

        // create a backend
        mpBackend = shared_ptr<BackendSlidingWindowG2O>(new BackendSlidingWindowG2O(mpTracker));
        mpTracker->SetBackEnd(mpBackend);

        // create a viewer
        mpViewer = shared_ptr<Viewer>(new Viewer());

        LOG(INFO) << "YGZ system all ready, waiting for images ..." << endl;
    }

    System::~System() {
        setting::destroySettings();
    }

    SE3d System::AddStereoIMU(
            const cv::Mat &imRectLeft, const cv::Mat &imRectRight,
            const double &timestamp, const VecIMU &vimu) {
        return mpTracker->InsertStereo(imRectLeft, imRectRight, timestamp, vimu);
    }

}